#include "aicp_lcm/drawingUtils.hpp"

void drawPointCloudCollections(boost::shared_ptr<lcm::LCM> &lcm, int index, Eigen::Isometry3d& pose, pcl::PointCloud<pcl::PointXYZRGB>& pcl_cloud, long long int utime, std::string pc_name_root)
{
  pronto_vis* pc_vis;
  pc_vis = new pronto_vis( lcm->getUnderlyingLCM() );

  int reset = 1;
  // Names
  std::stringstream pc_name;
  pc_name << pc_name_root << " ";
  pc_name << to_string(index);
  std::stringstream frame_name;
  frame_name << "PC Frame ";
  frame_name << to_string(index);
  // Indexes
  int pc_index = index+100;
  int frame_index = index;
  // Color
  srand (time(NULL));
  float r = (rand() % 101)/100.0;
  float g = (rand() % 101)/100.0;
  float b = (rand() % 101)/100.0;
  // obj: id name type reset
  // pts: id name type reset objcoll usergb rgb
  pc_vis->obj_cfg_list.push_back( obj_cfg(frame_index,frame_name.str().c_str(),5,reset) );
  pc_vis->ptcld_cfg_list.push_back( ptcld_cfg(pc_index,pc_name.str().c_str(),1,1,0,-1,{0.0,0.0,0.0})); // use RGB as randomly generated by viewer (uergb=-1)

  // Publish cloud and frame:
  Isometry3dTime poseT = Isometry3dTime(utime, pose);
  pc_vis->pose_to_lcm_from_list(frame_index, poseT);
  pc_vis->ptcld_to_lcm_from_list(pc_index, pcl_cloud, utime, utime);
}

void drawPointCloudNormalsCollections(boost::shared_ptr<lcm::LCM> &lcm, int index, Eigen::Isometry3d& pose, pcl::PointCloud<pcl::PointXYZRGBNormal>& pcl_cloud, long long int utime, std::string pc_name_root)
{
  pronto_vis* pc_vis;
  pc_vis = new pronto_vis( lcm->getUnderlyingLCM() );
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_tmp (new pcl::PointCloud<pcl::PointXYZRGB>);

  int reset = 1;
  // Names
  std::stringstream pc_name;
  pc_name << pc_name_root << " ";
  pc_name << to_string(index);
  std::stringstream frame_name;
  frame_name << "PC Frame ";
  frame_name << to_string(index);
  // Indexes
  int pc_index = index+1;
  int frame_index = index;

  Isometry3dTime poseT = Isometry3dTime(utime, pose);
  pc_vis->pose_to_lcm_from_list(frame_index, poseT);

  // Create normals list
  std::vector<Eigen::Vector3d> normals_list;
  for (size_t i = 0; i < pcl_cloud.points.size(); i++){
    Eigen::Vector4f p = pcl_cloud.points[i].getNormalVector4fMap ();
    normals_list.push_back ( Eigen::Vector3d( p[0],p[1],p[2] ) ) ;
  }
  pcl::copyPointCloud(pcl_cloud, *cloud_tmp);

  // obj: id name type reset
  // pts: id name type reset objcoll usergb rgb
  obj_cfg oconfig = obj_cfg(frame_index,frame_name.str().c_str(),5,reset);
  pc_vis->pose_to_lcm(oconfig, poseT);
  ptcld_cfg pconfig = ptcld_cfg(pc_index, pc_name.str().c_str(), 1, 1, frame_index, 0, {0.1,0.7,0.1});
  pc_vis->ptcld_to_lcm(pconfig, *cloud_tmp, normals_list, 0, 0);
}

void drawFrameCollections(boost::shared_ptr<lcm::LCM> &lcm, int index, Eigen::Isometry3d& pose, long long int utime, std::string frame_name_root)
{
  pronto_vis* pc_vis;
  pc_vis = new pronto_vis( lcm->getUnderlyingLCM() );

  int reset =1;
  // Name
  std::stringstream frame_name;
  frame_name << frame_name_root << " ";
  frame_name << to_string(index);
  // obj: id name type reset
  pc_vis->obj_cfg_list.push_back( obj_cfg(index,frame_name.str().c_str(),5,reset) );

  // Publish frame:
  Isometry3dTime poseT = Isometry3dTime(utime, pose);
  pc_vis->pose_to_lcm_from_list(index, poseT);
}

void HSVtoRGB( float &r, float &g, float &b, float h, float s, float v )
{
  int i;
  float f, p, q, t;

  if( s == 0 ) {
    // achromatic (grey)
    r = g = b = v;
    return;
  }

  h /= 60;      // sector 0 to 5
  i = floor( h );
  f = h - i;      // factorial part of h
  p = v * ( 1 - s );
  q = v * ( 1 - s * f );
  t = v * ( 1 - s * ( 1 - f ) );

  switch( i ) {
    case 0:
      r = v;
      g = t;
      b = p;
    break;
    case 1:
      r = q;
      g = v;
      b = p;
      break;
    case 2:
      r = p;
      g = v;
      b = t;
      break;
    case 3:
      r = p;
      g = q;
      b = v;
      break;
    case 4:
    r = t;
      g = p;
    b = v;
    break;
    default:    // case 5:
      r = v;
      g = p;
      b = q;
    break;
  }
}

void publishOctreeToLCM(boost::shared_ptr<lcm::LCM> &lcm, octomap::ColorOcTree* tree, string octree_channel){
  octomap_raw_t msg;
  msg.utime = bot_timestamp_now();

  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      msg.transform[i][j] = 0;
    }
    msg.transform[i][i] = 1;
  }

  std::stringstream datastream;
  tree->write(datastream);
  std::string datastring = datastream.str();
  msg.data = (uint8_t *) datastring.c_str();
  msg.length = datastring.size();

  octomap_raw_t_publish(lcm->getUnderlyingLCM(), octree_channel.c_str(), &msg);
}
